﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ObjectFinder.cs
 * \brief	This file contains the ObjectFinder function that 
 *  
 * This file contains the ObjectFinder function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.02
 * \date      2009-02-03
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                         2010-03-11 \n
 *      First Implementation of KalmanDynObj class
 *      
/* ######################################################################### */
using System;
using System.Collections.Generic;
using System.Text;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/*This is for using lists*/
using System.Collections;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using of input output library.*/
using System.IO;

/*Using the Globalization library.*/
using System.Globalization;

using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_ObjectFinder
     * 
     *  \brief  This is the class of the M44_ObjectFinder function.
     *
     */
    /* ------------------------------------------------------------------------- */
    [Serializable]
    [DataContract]
    [DataMemberConstructor]
    public class M44_KFDynObj
    {

        //Kalman Filter constants
        public bool _first = true;
        public double _INC_TIME = 0.250;
        public  double _cP_Pos = 0.01;
        public  double _cP_Size = 0.01;
        public  double _cP_Color = 0.01;
        public  double _cP_Temp = 0.01;

        public  double _cQ_Pos = 0.1;
        public  double _cQ_Size = 0.1;
        public  double _cQ_Color = 0.1;
        public  double _cQ_Temp = 0.1;

        public  double _cR_Pos = 0.1;
        public  double _cR_Size = 0.1;
        public  double _cR_Color = 0.1;
        public  double _cR_Temp = 0.1;
        

        //Kalman Filter Variables
        public  Matrix _Xk = new Matrix();
        public  Matrix _Xk_ant = new Matrix();
        public  Matrix _Xk_ant2 = new Matrix();
        public  Matrix _Pk = new Matrix();
        public  Matrix _Zk = new Matrix();          // Matrix with all the measurement vectors (nvar rows * n detected objects columns)
        public  Matrix _Xpred = new Matrix();
        public  Matrix _Ppred = new Matrix();
        public  Matrix _Xk_sub = new Matrix();
        public  Matrix _Pk_sub = new Matrix();
        public  Matrix _Xpred_sub = new Matrix();   //part of the X predicted
        public  Matrix _Ppred_sub = new Matrix();   //part of the P predicted
        public  Matrix _Zk_sub = new Matrix();       //part of the measurement vector

        public  Matrix _TimeDetection = new Matrix(); //Time since detection
        public  Matrix _ObjectID = new Matrix(); //Object ID
        public  double _maxID = 0;
        public  float _LastTime = 0.0f;

        public  int _N_OBJ = 0;
        public  int _N_VAR = 7; //Number of variables in each object state
        public  int _N_U = 2;

        public  Matrix _ObjNotDetected = new Matrix();
        public  Matrix _NewObjDetected = new Matrix();

        //Equation variables
        public  Matrix _Ak = new Matrix();
        public  Matrix _Bk = new Matrix();
        public  Matrix _Hk_sub = new Matrix();
        public  Matrix _Uk = new Matrix();
        public  Matrix _Yk_sub = new Matrix();
        public  Matrix _Sk_sub = new Matrix();
        public  Matrix _Kk_sub = new Matrix();


        public  Matrix _Qk = new Matrix();
        public  Matrix _Rk = new Matrix();
        public  Matrix _Rk_sub = new Matrix();


        //Colors
        /*
                black     = 0,RGB: 0,0,0
                grey      = 1,RGB: 190,190,190
                blue      = 2,RGB: 0,0,255
                green     = 3, RGB:0,255,0
                red       = 4, RGB:255,0,0
                yellow    = 5, RGB:255,255,0
                white     = 6 ,RG: 255,255,0
        */
        //Kalman Filter Functions
        public M44_KFDynObj() { }

        /* ------------------------------------------------------------------------- */
        /**	\fn public 
         * 
         *  \brief 
         *  
         *  \param[in] 
         *  
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */
        public void InitializeFilter(cl_DynamicObjects DynObjs,char[] RobotID)
        {
            
            int n_states = _N_VAR * DynObjs.us_Size;
            int n_aux1 = 1;
            int n_aux2 = 0;
            bool first = true;
            _N_OBJ = DynObjs.us_Size;
            _Xk = new Matrix(n_states, 1);
            _TimeDetection = Matrix.Zeros(DynObjs.us_Size, 1);
            _ObjectID = Matrix.Zeros(DynObjs.us_Size, 1);
            _Pk = new Matrix(n_states, n_states);
            _Rk = new Matrix(n_states, n_states);
            _Ak = Matrix.Identity(n_states);
            _Bk = new Matrix(n_states, 2 * DynObjs.us_Size);
            _Qk = new Matrix(n_states, n_states);
            _Uk = new Matrix(2 * DynObjs.us_Size, 1);
            int n1 = Convert.ToInt32(Convert.ToString(RobotID[7]));
            int n2 = Convert.ToInt32(Convert.ToString(RobotID[8]));
            double Obj_ID = (double)(n1 * 10000 + n2 * 1000);
            for (int i = 0; i < DynObjs.us_Size; i++)
            {
                _Xk[i * _N_VAR + 1, 1].Re = DynObjs.List[i].Position.d_x;
                _Xk[i * _N_VAR + 2, 1].Re = DynObjs.List[i].Position.d_y;
                _Xk[i * _N_VAR + 3, 1].Re = DynObjs.List[i].PhysCharac.f_PhysSize;
                _Xk[i * _N_VAR + 4, 1].Re = 0;// DynObjs.List[i].PhysCharac.e_ObjectColor;  //R from RGB
                _Xk[i * _N_VAR + 5, 1].Re = 0;//DynObjs.List[i].PhysCharac.e_ObjectColor;  //G from RGB
                _Xk[i * _N_VAR + 6, 1].Re = 0;//DynObjs.List[i].PhysCharac.e_ObjectColor;  //B from RGB
                _Xk[i * _N_VAR + 7, 1].Re = 0; // Temperatura
                //Object ID
                _ObjectID[i+1,1].Re = (Obj_ID+1);
                Obj_ID++;
            }
            _maxID = Obj_ID;

            for (int i = 1; i <= n_states; i++)
            {
                for (int j = 1; j <= n_states; j++)
                {
                    if (i == j)
                    {
                        switch (n_aux1)
                        {
                            case 1:
                            case 2: _Pk[i, j].Re = _cP_Pos; _Qk[i, j].Re = _cQ_Pos; _Rk[i, j].Re = _cR_Pos; break;
                            case 3: _Pk[i, j].Re = _cP_Size; _Qk[i, j].Re = _cQ_Size; _Rk[i, j].Re = _cR_Size; break;
                            case 4:
                            case 5:
                            case 6: _Pk[i, j].Re = _cP_Color; _Qk[i, j].Re = _cQ_Color; _Rk[i, j].Re = _cR_Color; break;
                            case 7: _Pk[i, j].Re = _cP_Temp; _Qk[i, j].Re = _cQ_Temp; _Rk[i, j].Re = _cR_Temp; break;
                            default: break;
                        }
                        n_aux1++;
                        if (n_aux1 == _N_VAR + 1)
                            n_aux1 = 1;
                    }

                }
                for (int j = 1; j <= _N_U * DynObjs.us_Size; j++)
                {
                    switch (i - n_aux2 * _N_VAR)
                    {
                        case 1:
                            if (j == (1 + n_aux2 * _N_U))
                            {
                                _Bk[i, j].Re = _INC_TIME; 
                            }
                            break;
                        case 2:
                            if (j == (2 + n_aux2 * _N_U))
                            {
                                _Bk[i, j].Re = _INC_TIME;
                            }
                            break;
                        default:
                            _Bk[i, j].Re = 0; break;
                            
                    }
                    if (first)
                    {
                        _Uk[j].Re = 0;
                    }
                }
                if (i - n_aux2 * _N_VAR >= _N_VAR)
                    n_aux2++;
            }
            first = false;
            _Xk_ant = _Xk;
        }
      
        /* ------------------------------------------------------------------------- */
        /**	\fn public ComputePrediction()
         * 
         *  \brief Compute the prediction based on the state or the covariance matrix
         */
        /* ------------------------------------------------------------------------- */
        public void ComputePrediction()
        {

            _Xpred = _Ak * _Xk + _Bk * _Uk;
            _Ppred = _Ak * _Pk * _Ak.Transpose() + _Qk;

        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public Matrix Pairing(cl_DynamicObjects DynObjs)
         * 
         *  \brief Pairing between the measurement DynObjs and the state vector
         *  \param[in] cl_DynamicObjects DynObjs
         *  \param[out] Matrix Selected Pairs
         *
         */
        /* ------------------------------------------------------------------------- */

        public  Matrix Pairing(cl_DynamicObjects DynObjs)
        {
            Matrix X_sub = new Matrix(_N_VAR, 1);
            Matrix P_sub = new Matrix(_N_VAR, 1);
            Matrix Z_sub = new Matrix(_N_VAR, 1);
            bool first = true;
            _Zk = new Matrix();
            Matrix Posible_Pairs = new Matrix(_N_OBJ*DynObjs.us_Size,3);
            Matrix Sel_Pairs = new Matrix(Math.Min(_N_OBJ,DynObjs.us_Size),3);
            //Compute the values of the distance between all the objects
            for (int i = 1; i <= _N_OBJ; i++)
            {
                for (int k = 1; k <= _N_VAR; k++)
                {
                    X_sub[k, 1] = _Xk[k + (i - 1) * _N_VAR, 1];
                    P_sub[k, 1] = _Pk[k + (i - 1) * _N_VAR, k + (i - 1) * _N_VAR];
                }
                for (int j = 0; j < DynObjs.us_Size; j++)
                {
                    Z_sub[1, 1].Re = DynObjs.List[j].Position.d_x;
                    Z_sub[2, 1].Re = DynObjs.List[j].Position.d_y;
                    
                    if (DynObjs.List[j].Position.d_x < -118)
                    {
                        Z_sub[3, 1].Re = 5;// DynObjs.List[j].PhysCharac.f_PhysSize;
                        Z_sub[4, 1].Re = 255;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
                        Z_sub[5, 1].Re = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
                        Z_sub[6, 1].Re = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
                        Z_sub[7, 1].Re = 28;  //TEMP
                    }
                    else
                    {
                        Z_sub[3, 1].Re = 10;// DynObjs.List[j].PhysCharac.f_PhysSize;
                        Z_sub[4, 1].Re = 0;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
                        Z_sub[5, 1].Re = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
                        Z_sub[6, 1].Re = 255;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
                        Z_sub[7, 1].Re = 15;  //TEMP
                    }
                    if (first)
                    {
                        _Zk.Insert(_Zk.RowCount + 1, 1, Matrix.Ones(_N_VAR,1));
                        _Zk[1 + j * _N_VAR, 1].Re = DynObjs.List[j].Position.d_x;
                        _Zk[2 + j * _N_VAR, 1].Re = DynObjs.List[j].Position.d_y;
                        if (DynObjs.List[j].Position.d_x < -118)
                        {
                            _Zk[3 + j * _N_VAR, 1].Re = 5;// DynObjs.List[j].PhysCharac.f_PhysSize;
                            _Zk[4 + j * _N_VAR, 1].Re = 255;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
                            _Zk[5 + j * _N_VAR, 1].Re = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
                            _Zk[6 + j * _N_VAR, 1].Re = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
                            _Zk[7 + j * _N_VAR, 1].Re = 28;  //TEMP
                        }
                        else
                        {
                            _Zk[3 + j * _N_VAR, 1].Re = 10;// DynObjs.List[j].PhysCharac.f_PhysSize;
                            _Zk[4 + j * _N_VAR, 1].Re = 0;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
                            _Zk[5 + j * _N_VAR, 1].Re = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
                            _Zk[6 + j * _N_VAR, 1].Re = 255;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
                            _Zk[7 + j * _N_VAR, 1].Re = 15;  //TEMP
                        }
                    }
                    //Calculate Mahalanobis Distance
                    Posible_Pairs[(j + 1) + (i-1) * DynObjs.us_Size, 1].Re = i;
                    Posible_Pairs[(j + 1) + (i-1) * DynObjs.us_Size, 2].Re = j;
                    Posible_Pairs[(j + 1) + (i-1) * DynObjs.us_Size, 3].Re = MahalanobisDist(X_sub,Z_sub,P_sub);
                }
                first = false;
            }
            //Find the minimum distance pairs

            Sel_Pairs[1,1] = Posible_Pairs[1,1];
            Sel_Pairs[1,2] = Posible_Pairs[1,2];
            Sel_Pairs[1,3] = Posible_Pairs[1,3];
            for(int i = 1;i<=Math.Min(_N_OBJ,DynObjs.us_Size);i++)
            {
                for(int j = 1;j<=_N_OBJ*DynObjs.us_Size;j++)
                {
                    if ((Sel_Pairs[i,3].Re > Posible_Pairs[j,3].Re && Posible_Pairs[j,3].Re>0) || Sel_Pairs[i,3].Re == -1)
                    {
                        Sel_Pairs[i,1] = Posible_Pairs[j,1];
                        Sel_Pairs[i,2] = Posible_Pairs[j,2];
                        Sel_Pairs[i,3] = Posible_Pairs[j,3];
                    }
                }
                for (int j = 1;j<=_N_OBJ*DynObjs.us_Size;j++)
                {
                    if (Sel_Pairs[i,1] == Posible_Pairs[j,1])
                        Posible_Pairs[j,3].Re = -1;
                    if (Sel_Pairs[i,2] == Posible_Pairs[j,2])
                        Posible_Pairs[j,3].Re = -1;

                }
                if (i + 1 <= Math.Min(_N_OBJ, DynObjs.us_Size))
                    Sel_Pairs[i+1,3].Re = -1;
            }

            //Eliminate the pairs with distances too big
            for (int i = 1; i <= Sel_Pairs.RowCount; i++)
            {
                if(Sel_Pairs[i,3].Re>50)
                {
                    Sel_Pairs.ExtractRow(i);
                }
            }
            return Sel_Pairs;



        }
        /* ------------------------------------------------------------------------- */
        /**	\fn public MahalanobisDist(Matrix X_sub, Matrix Z_sub, Matrix P_sub)
         * 
         *  \brief Calculate the Mahalanobis Distance between the one object and one measurement
         *  
         *  \param[in] Matrix X_sub State Vector
         *  \param[in] matrix Z_sub Measurements
         *  \param[in] Matrix P_sub Covariances
         *  \param[out] double Mahalanobis distance
         *
         */
        /* ------------------------------------------------------------------------- */

        public  double MahalanobisDist(Matrix X_sub, Matrix Z_sub, Matrix P_sub)
        {
            double sum = 0;
            double aux1 = 0;
            double aux2 = 0;
            double MahDist = 0;
            for (int i = 1; i <= _N_VAR; i++)
            {
                aux1 = Math.Pow(X_sub[i,1].Re-Z_sub[i,1].Re,2);
                aux2 = Math.Pow(P_sub[i,1].Re,2);
                sum = sum + (aux1 / aux2);
            }
            MahDist = Math.Sqrt(sum);
            return MahDist;

        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public PrepareFilterMatrix(Matrix Sel_Pairs,cl_DynamicObjects DynObjs)
         * 
         *  \brief Calculate the Mahalanobis Distance between the one object and one measurement
         *  
         *  \param[in] Matrix Sel_Pairs
         *  \param[in] cl_DynamicObjects DynObjs
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */

        public  void PrepareFilterMatrix(Matrix Sel_Pairs,cl_DynamicObjects DynObjs)
        {
            
            int i_x, i_z;
            _Xpred_sub = new Matrix(Sel_Pairs.RowCount * _N_VAR, 1);
            _Ppred_sub = new Matrix(Sel_Pairs.RowCount * _N_VAR, Sel_Pairs.RowCount * _N_VAR);
            _Zk_sub = new Matrix(Sel_Pairs.RowCount * _N_VAR, 1);
            _Hk_sub = Matrix.Identity(Sel_Pairs.RowCount * _N_VAR);
            _Rk_sub = Matrix.Zeros(Sel_Pairs.RowCount * _N_VAR);

            //Include the filter pairs first
            for (int k9 = 1; k9 <= Sel_Pairs.RowCount; k9++)
            {
                i_x = (int)Sel_Pairs[k9,1].Re;
                i_z = (int)Sel_Pairs[k9,2].Re;
                for (int j = 1;j<= _N_VAR;j++)
                {
                    _Xpred_sub[j + (k9 - 1) * _N_VAR , 1] = _Xpred[j + (i_x - 1) * _N_VAR, 1];
                    _Ppred_sub[j + (k9 - 1) * _N_VAR , j + (k9 - 1) * _N_VAR] = _Ppred[j + (i_x - 1) * _N_VAR, j + (i_x - 1) * _N_VAR];
                    _Zk_sub[j + (k9 - 1) * _N_VAR, 1] = _Zk[j + (i_z * _N_VAR), 1];
                    _Rk_sub[j + (k9 - 1) * _N_VAR, j + (k9 - 1) * _N_VAR] = _Rk[j + (i_z * _N_VAR), j + (i_z * _N_VAR)];
                }
            }
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public ComputeFilter()
         * 
         *  \brief 
         *  
         *  \param[in] 
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */

        public  void ComputeFilter()
        {
            Matrix Iden = new Matrix(); 
            _Xk_sub = new Matrix();
            _Pk_sub = new Matrix();
            _Yk_sub = new Matrix();
            _Sk_sub = new Matrix();
            _Kk_sub = new Matrix();

            _Yk_sub = _Zk_sub - _Hk_sub * _Xpred_sub;
            _Sk_sub = _Hk_sub * _Ppred_sub * _Hk_sub.Transpose() + _Rk_sub;
            _Kk_sub = _Ppred_sub * _Hk_sub.Transpose() * _Sk_sub.Inverse();
            Iden = Matrix.Identity(_Kk_sub.ColumnCount);
            _Xk_sub = _Xpred_sub + _Kk_sub * _Yk_sub;
            _Pk_sub = (Iden - _Kk_sub * _Hk_sub) * _Ppred_sub;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public RepairFilterMatrix()
         * 
         *  \brief 
         *  
         *  \param[in] 
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */

        public  void RepairFilterMatrix(Matrix Sel_Pairs,cl_DynamicObjects DynObjs)
        {
            int i_sub1, i_sub2,i_sub3;
            int i_x;
            Matrix ObjAdded = new Matrix();
            Matrix X_aux = new Matrix(_N_VAR,1);
            Matrix P_aux = new Matrix(1,1);
            Matrix B_aux = new Matrix();
            Matrix Q_aux = new Matrix();
            Matrix R_aux = new Matrix();
            Matrix Time_aux = new Matrix();
            Matrix ID_aux = new Matrix(1,1);
            Matrix M_aux = new Matrix();
            Matrix IncTime = new Matrix();
            Matrix Maux1 = new Matrix();
            Matrix Maux2 = new Matrix();

            bool add_object = true;

            //Make copies of the real variables
            B_aux = _Bk;
            Q_aux = _Qk;
            R_aux = _Rk;
            Time_aux = _TimeDetection;
            ID_aux = _ObjectID;

            //First we include the objects computed in the filter
            if (Sel_Pairs.RowCount > 0)
            {
                //Empty the actual variables
                _Xk = new Matrix();
                _Pk = new Matrix();
                _Bk = new Matrix();
                _Qk = new Matrix();
                _Rk = new Matrix();
                _TimeDetection = new Matrix();
                _ObjectID = new Matrix(Sel_Pairs.RowCount, 1);
                //Save the recently computed values
                _Xk = _Xk_sub;
                _Pk = _Pk_sub;
                _Ak = Matrix.Identity(Sel_Pairs.RowCount * _N_VAR);
                _TimeDetection = Matrix.Zeros(Sel_Pairs.RowCount, 1); //Time since detection

                for (int i = 1; i <= Sel_Pairs.RowCount; i++)
                {
                    i_x = (int)Sel_Pairs[i, 1].Re;
                    i_sub1 = 1 + (i_x - 1) * _N_VAR;
                    i_sub2 = _N_VAR + (i_x - 1) * _N_VAR;
                    i_sub3 = 1 + (i_x - 1) * _N_U;
                    Maux1 = Q_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2);
                    Maux2 = B_aux.Extract(i_sub1, i_sub2, i_sub3, i_sub3 + 1);
                    _Qk.Insert(1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_VAR, Q_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
                    _Bk.Insert(1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_U, B_aux.Extract(i_sub1, i_sub2, i_sub3, i_sub3 + 1));
                    _Rk.Insert(1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_VAR, R_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
                    _ObjectID[i, 1].Re = ID_aux[i_x, 1].Re;
                }
            }
            // Insert old objects not seen this time
            ObjAdded = Sel_Pairs.Column(1);
            _ObjNotDetected = new Matrix();
            IncTime = new Matrix(1, 1);
            IncTime[1, 1].Re = _INC_TIME;
            if (_Xpred.RowCount / _N_VAR > Sel_Pairs.RowCount)
            {
                
                for (int i = 1; i <= _Xpred.RowCount / _N_VAR; i++)
                {
                    add_object = true;
                    for (int j = 1; j <= ObjAdded.RowCount; j++)
                    {
                        if (i == ObjAdded[j, 1].Re)
                        {
                            add_object = false;
                            break;
                        }
                    }
                    //Check if the object has not been seen in a too much time
                    M_aux = Time_aux.Extract(i, i, 1, 1);
                    if (M_aux[1, 1].Re > 3)  //si el objeto no se ha visto mas de 3 segundos
                    {
                        add_object = false;
                    }
                    if (add_object)
                    {
                        _ObjNotDetected.Insert(_ObjNotDetected.RowCount + 1, 1, i * Matrix.Identity(1));
                        i_sub1 = (i - 1) * _N_VAR + 1;
                        i_sub2 = (i - 1) * _N_VAR + _N_VAR;
                        i_sub3 = 1 + (i - 1) * _N_U;
 
                        _Xk.Insert(_Xk.RowCount + 1, 1, _Xpred.Extract(i_sub1,i_sub2 , 1, 1));
                        _Pk.Insert(_Pk.RowCount+1, _Pk.ColumnCount+1, _Ppred.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
                        _Ak.Insert(_Ak.RowCount+1, _Ak.ColumnCount+1, Matrix.Identity(_N_VAR));
                        _Qk.Insert(_Qk.RowCount+1,_Qk.ColumnCount+1,Q_aux.Extract(i_sub1,i_sub2,i_sub1,i_sub2));
                        _Rk.Insert(_Rk.RowCount + 1, _Rk.ColumnCount + 1, R_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
                        _Bk.Insert(_Bk.RowCount+1,_Bk.ColumnCount+1,B_aux.Extract(i_sub1,i_sub2,i_sub3,i_sub3+1));
                        
                        _TimeDetection.Insert(_TimeDetection.RowCount+1,1, Time_aux.Extract(i,i,1,1) + IncTime);
                        _ObjectID.Insert(_ObjectID.RowCount + 1, 1, ID_aux.Extract(i, i, 1, 1));
                    }
                }
            }

            //Insert new objects, if any
            ObjAdded = Sel_Pairs.Column(2);
            //Create P_aux and Q_aux and R_aux and B_aux with standard valeus for P and Q and R and B
            P_aux = new Matrix();
            P_aux = _cP_Pos * Matrix.Identity(2);
            P_aux.Insert(P_aux.RowCount + 1, P_aux.ColumnCount + 1, _cP_Size * Matrix.Identity(1));
            P_aux.Insert(P_aux.RowCount + 1, P_aux.ColumnCount + 1, _cP_Color * Matrix.Identity(3));
            P_aux.Insert(P_aux.RowCount + 1, P_aux.ColumnCount + 1, _cP_Temp * Matrix.Identity(1));
            Q_aux = new Matrix();
            Q_aux = _cQ_Pos * Matrix.Identity(2);
            Q_aux.Insert(Q_aux.RowCount + 1, Q_aux.ColumnCount + 1, _cQ_Size * Matrix.Identity(1));
            Q_aux.Insert(Q_aux.RowCount + 1, Q_aux.ColumnCount + 1, _cQ_Color * Matrix.Identity(3));
            Q_aux.Insert(Q_aux.RowCount + 1, Q_aux.ColumnCount + 1, _cQ_Temp * Matrix.Identity(1));
            R_aux = new Matrix();
            R_aux = _cR_Pos * Matrix.Identity(2);
            R_aux.Insert(R_aux.RowCount + 1, R_aux.ColumnCount + 1, _cR_Size * Matrix.Identity(1));
            R_aux.Insert(R_aux.RowCount + 1, R_aux.ColumnCount + 1, _cR_Color * Matrix.Identity(3));
            R_aux.Insert(R_aux.RowCount + 1, R_aux.ColumnCount + 1, _cR_Temp * Matrix.Identity(1));
            B_aux = new Matrix();
            B_aux = _INC_TIME * Matrix.Identity(2);
            B_aux.Insert(B_aux.RowCount + 1, 1, Matrix.Zeros(_N_VAR-_N_U,_N_U));
            _NewObjDetected = new Matrix();
            if (Sel_Pairs.RowCount < DynObjs.us_Size)
            {
                add_object = true;
                for (int i = 0; i < DynObjs.us_Size; i++)
                {
                    for (int j = 1; j <= ObjAdded.RowCount; j++)
                    {
                        if (i == ObjAdded[j, 1])
                        {
                            add_object = false;
                            break;
                        }
                    }
                    if (add_object)
                    {
                        _NewObjDetected.Insert(_NewObjDetected.RowCount + 1, 1, (i + 1) * Matrix.Identity(1));
                        _Xk.Insert(_Xk.RowCount + 1, 1, _Zk.Column(i+1));
                        _Pk.Insert(_Pk.RowCount + 1, _Pk.ColumnCount + 1, P_aux);
                        _Ak.Insert(_Ak.RowCount + 1, _Ak.ColumnCount + 1, Matrix.Identity(_N_VAR));
                        _Qk.Insert(_Qk.RowCount + 1, _Qk.ColumnCount + 1, Q_aux);
                        _Bk.Insert(_Bk.RowCount + 1, _Bk.ColumnCount + 1, B_aux);
                        _Rk.Insert(_Rk.RowCount + 1, _Rk.ColumnCount + 1, R_aux);
                        _TimeDetection.Insert(_TimeDetection.RowCount + 1, 1, Matrix.Zeros(1,1));
                        M_aux = new Matrix(1, 1);
                        M_aux[1,1].Re = (double)(_maxID + 1);
                        _maxID++;
                        _ObjectID.Insert(_ObjectID.RowCount + 1, 1, M_aux);
                    }
                }
            }
            
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public ComputeU()
         * 
         *  \brief 
         *  
         *  \param[in] 
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */
        public  void ComputeU(Matrix Sel_Pairs)
        {
            _Uk = new Matrix(2 * _Xk.RowCount / _N_VAR, 1);
            int i = 1;
            while(i<=_Xk.RowCount/_N_VAR)
            {
                if (i <= Sel_Pairs.RowCount)
                {
                    _Uk[1 + (i - 1) * _N_U, 1] = (_Xk[1 + (i - 1) * _N_VAR, 1] - _Xk_ant[1 + (int)(Sel_Pairs[i, 1].Re - 1) * _N_VAR, 1])/_INC_TIME;
                    _Uk[2 + (i - 1) * _N_U, 1] = (_Xk[2 + (i - 1) * _N_VAR, 1] - _Xk_ant[2 + (int)(Sel_Pairs[i, 1].Re - 1) * _N_VAR, 1])/_INC_TIME;
                    i++;
                }
                else
                {
                    for (int j = 1; j <= _ObjNotDetected.RowCount; j++)
                    {
                        _Uk[1 + (i - 1) * _N_U, 1] = (_Xk[1 + (i - 1) * _N_VAR, 1] - _Xk_ant[1 + (int)(_ObjNotDetected[j, 1].Re - 1) * _N_VAR, 1]) / _INC_TIME;
                        _Uk[2 + (i - 1) * _N_U, 1] = (_Xk[2 + (i - 1) * _N_VAR, 1] - _Xk_ant[2 + (int)(_ObjNotDetected[j, 1].Re - 1) * _N_VAR, 1]) / _INC_TIME;
                        i++;
                    }
                    for (int j = 1; j <= _NewObjDetected.RowCount; j++)
                    {
                        _Uk[1 + (i - 1) * _N_U, 1].Re = 0;
                        _Uk[2 + (i - 1) * _N_U, 1].Re = 0;
                        i++;
                    }
                }
            }
            _Xk_ant = new Matrix();
            _Xk_ant = _Xk;
       }

        /* ------------------------------------------------------------------------- */
        /**	\fn public ComputeIncTime()
         * 
         *  \brief 
         *  
         *  \param[in] 
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */
        public  void ComputeIncTime(float actTime)
        {
            _INC_TIME = actTime - _LastTime;
            _LastTime = actTime;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public KalmanFilterMain(cl_DynamicObjects DynObjs,float actTime)
         * 
         *  \brief 
         *  
         *  \param[in] cl_DynamicObjects DynObjs
         *  \param[in] float actTime
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */
        public cl_DynamicObjects KalmanFilterMain(cl_DynamicObjects DynObjs, float actTime, char[] RobotID)
        {
            Matrix Sel_Pairs = new Matrix();
            cl_DynamicObjects DynObjects_result = new cl_DynamicObjects();
            if (_first)
            {
                InitializeFilter(DynObjs,RobotID);
                ComputeIncTime(actTime);
                _first = false;
            }
            else
            {
                ComputeIncTime(actTime);
                ComputePrediction();
                if (DynObjs.us_Size > 0)
                {
                    Sel_Pairs = Pairing(DynObjs);
                    if (Sel_Pairs.RowCount > 0)
                    {
                        PrepareFilterMatrix(Sel_Pairs, DynObjs);
                        ComputeFilter();
                    }
                    else
                    {
                        Sel_Pairs = new Matrix();
                    }
                }
                else
                {
                    Sel_Pairs = new Matrix();
                }
                RepairFilterMatrix(Sel_Pairs, DynObjs);
                ComputeU(Sel_Pairs);
                
            }
            _N_OBJ = _Xk.RowCount / _N_VAR;
            //Change back to Dynamic Object Structure
            DynObjects_result = State2DynObjList(actTime);
            return DynObjects_result;
        }

        /* ------------------------------------------------------------------------- */
        /**	\fn public State2DynObjList()
         * 
         *  \brief 
         *  
         *  \param[in] 
         *  \param[out] 
         *
         */
        /* ------------------------------------------------------------------------- */
        public  cl_DynamicObjects State2DynObjList(float actTime)
        {
            //Create Dynamic Object List at the end of each Iteration
            cl_DynamicObjects DynamicObjects = new cl_DynamicObjects((ushort)_TimeDetection.RowCount);
            for (int i = 0; i < _TimeDetection.RowCount; i++)
            {
                DynamicObjects.List[i] = new cl_DynamicObjectList();
                DynamicObjects.List[i].f_DetectionTime = (float)(actTime - (_TimeDetection[i + 1, 1].Re));
                DynamicObjects.List[i].PhysCharac.f_PhysSize = (float)_Xk[3 + i * _N_VAR,1].Re;
                DynamicObjects.List[i].Position.d_x = _Xk[1 + i * _N_VAR, 1].Re;
                DynamicObjects.List[i].Position.d_y = _Xk[2 + i * _N_VAR, 1].Re;
                DynamicObjects.List[i].Velocity.d_xVel = _Uk[1 + i * _N_U, 1].Re;
                DynamicObjects.List[i].Velocity.d_yVel = _Uk[2 + i * _N_U, 1].Re;
                DynamicObjects.List[i].us_Identification = (ushort)_ObjectID[i + 1, 1].Re;
            }
            return DynamicObjects;
            
        }

    }
}


